import math
import serial
import struct
import time
import logging
from logging import handlers

log=logging.getLogger('arucoLog')
log.setLevel(logging.INFO)
fmt=logging.Formatter('%(asctime)s:%(levelname)s:%(funcName)s:%(message)s')
sh=logging.StreamHandler()
sh.setLevel(logging.INFO)
sh.setFormatter(fmt)
log.addHandler(sh)
#fh=logging.FileHandler("auto_fly_attack.log",'w')
fh=handlers.TimedRotatingFileHandler(filename='aruco.txt',when='D')
fh.setLevel(logging.INFO)
fh.setFormatter(fmt)
log.addHandler(fh)

def chooseValue(X,Y,val):# 从小到大
    for x,y in zip(X,Y):
        if val<=x:
            return y
    return Y[-1]
def safeSend(msg):
    ser = serial.Serial('COM5', 115200)
    for i in range(10):
        ser.write(msg)
        time.sleep(0.02)
# def safeSend(msg):
#     ser = serial.Serial('COM5', 115200,timeout=0.5)
#     ser.write(msg)
#     flag=False
#     while True:
#         r=ser.read(24)
#         if r==msg:
#             break;
#         log.warning(f"丢包{msg}")
#         flag=True
#         ser.write(msg)
#     log.info("发送成功")
#     return flag
    
def Move(endPos,Q,stop=True,vpara=([50,400,500,1e18],[100,100,500,500]),earlyStopRange=20,safeStop=False,targetAngle=None):
    log.info(f"endPos:{endPos}")
    angle,pos=Q.get()#丢弃
    ser = serial.Serial('COM5', 115200)
    ti=0
    tr=time.time() #转轮子需要时间
    stopTime=0
    while True:
        angle,pos=Q.get()
        if angle==-1926:
            log.info("None")
            time.sleep(0.01)
            continue
        if 800<=pos[0]<2500 and 2400<=pos[1]<3200:
            time.sleep(0.01)
            log.info(f"屏蔽{pos}")
            continue
        dist=math.sqrt((endPos[0]-pos[0])**2+(endPos[1]-pos[1])**2)
        # log.info(f'dist:{dist} pos:{pos} angle:{angle}')
        if stopTime!=0:#视觉负反馈ing
            if time.time()-stopTime>0.2:
                if dist>earlyStopRange:
                    log.warning("没刹住,丢包?")
                    stopTime=0#重来
                else:
                    log.info("刹车完成")
                    break
            else:
                ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                continue
        if dist<=earlyStopRange:
            if stop:
                # ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                log.info(f"平移提前刹车 dist:{dist} pos:{pos}")
                ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                if safeStop:
                    stopTime=time.time()
                    continue
                else:
                    break
            else:
                break

        if time.time()-ti>=0.02:#控制一次的间隔
            ti=time.time()
            newAngle=math.atan2(endPos[1]-pos[1],endPos[0]-pos[0])
            if time.time()-tr<=0.02:
                v=0
            else:
                v=chooseValue(vpara[0],vpara[1],dist)
            wheelAngle=newAngle-angle
            while wheelAngle<0:
                wheelAngle+=math.pi*2 #[0,2pi]
            log.info(f'wheelAngle:{wheelAngle} dist:{dist} v:{v} pos:{pos}')
            ser.write(struct.pack('iffiff',0,-v,wheelAngle,0,0,0))
        # time.sleep(0.01)
    # while True:
    #     angle=mangle.value
    #     pos=mpos[:]
    #     if angle==-1926:
    #         log.info("None")
    #         time.sleep(0.1)
    #         continue
    #     dist=abs(endPos[0]-pos[0])+abs(endPos[1]-pos[1])
    #     log.info(f'dist:{dist} pos:{pos}')
    #     time.sleep(0.01)
    ser.close()
def controlRotate(targetAngle,Q,timeout=0.0):#sleep?
    angle,pos=Q.get()#丢弃
    ser = serial.Serial('COM5', 115200)
    # pack=struct.pack('iffiff',1,0,0,0,200,0)
    # ser.write(pack)
    lastv=0
    while True:
        angle,pos=Q.get()
        if angle==-1926:
            print("None")
            time.sleep(0.01)
            continue
        try:
            if angle<targetAngle:
                if targetAngle-angle<angle+2*math.pi-targetAngle:
                    dist=targetAngle-angle
                    direction=-1
                else:
                    dist=angle+2*math.pi-targetAngle
                    direction=1
            else:
                if angle-targetAngle<targetAngle+2*math.pi-angle:
                    dist=angle-targetAngle
                    direction=1
                else:
                    dist=targetAngle+2*math.pi-angle
                    direction=-1
            if abs(lastv)==500 and abs(dist)<=0.24:# 前馈
                log.info("旋转前馈刹车 500速")
                ser.write(struct.pack('iffiff',2,0,0,0,0,0))
                break
            dist=dist*180/math.pi
            v=chooseValue([15,360],[50,500,500],dist)*direction
            log.info(f'dist {dist} deg v {v} angle {angle}')
            if dist<=1:
                ser.write(struct.pack('iffiff',2,0,0,0,0,0))
                break
            ser.write(struct.pack('iffiff',1,0,0,0,v,0))
            # if v!=lastv:
            #     ser.write(struct.pack('iffiff',1,0,0,0,v,0))
            #     lastv=v
            # time.sleep(0.003)
        except Exception as e:
            log.info(e)
    time.sleep(timeout)

def Calculate(endPos,Q,targetAngle=0,stop=True,vpara=([50,400,500,1e18],[100,100,500,500]),wpara=([1,15,45,365],[0,50,200,400]),earlyStopRange=20,safeStop=False):
    log.info(f"endPos:{endPos}")
    angle,pos=Q.get()#丢弃
    ser = serial.Serial('COM5', 115200)
    ti=0
    tr=time.time() #转轮子需要时间
    stopTime=0
    while True:
        angle,pos=Q.get()
        if angle==-1926:
            log.info("None")
            time.sleep(0.01)
            continue
        if 800<=pos[0]<2000 and 2400<=pos[1]<3200:
            time.sleep(0.01)
            log.info(f"屏蔽{pos}")
            continue
        dist=math.sqrt((endPos[0]-pos[0])**2+(endPos[1]-pos[1])**2)
        log.info(f'dist:{dist} pos:{pos} angle:{angle}')
        if stopTime!=0:#视觉负反馈ing
            if time.time()-stopTime>0.3:
                if dist>earlyStopRange:
                    log.warning("没刹住,丢包?")
                    stopTime=0#重来
                else:
                    log.info("刹车完成")
                    break
            else:
                ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                continue
        if dist<=earlyStopRange:
            if stop:
                # ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                log.info(f"速度解算提前刹车 dist:{dist} pos:{pos}")
                ser.write(struct.pack('iffiff',0,0,0,0,0,0))
                if safeStop:
                    stopTime=time.time()
                    continue
                else:
                    break
            else:
                break

        if time.time()-ti>=0.02:#控制一次的间隔
            ti=time.time()
            if time.time()-tr<=0.02:
                v=0
            else:
                v=chooseValue(vpara[0],vpara[1],dist)
            newAngle=math.atan2(endPos[1]-pos[1],endPos[0]-pos[0])
            if angle<targetAngle:
                if targetAngle-angle<angle+2*math.pi-targetAngle:
                    angDist=targetAngle-angle
                    direction=-1
                else:
                    angDist=angle+2*math.pi-targetAngle
                    direction=1
            else:
                if angle-targetAngle<targetAngle+2*math.pi-angle:
                    angDist=angle-targetAngle
                    direction=1
                else:
                    angDist=targetAngle+2*math.pi-angle
                    direction=-1
            w=chooseValue(wpara[0],wpara[1],angDist/math.pi*180)*direction
            log.info(f'v:{v} w:{w}')
            ser.write(struct.pack('iffiff',18,-v,newAngle,0,w,angle))
        # time.sleep(0.01)
    # while True:
    #     angle=mangle.value
    #     pos=mpos[:]
    #     if angle==-1926:
    #         log.info("None")
    #         time.sleep(0.1)
    #         continue
    #     dist=abs(endPos[0]-pos[0])+abs(endPos[1]-pos[1])
    #     log.info(f'dist:{dist} pos:{pos}')
    #     time.sleep(0.01)
    ser.close()

def controlSuckerOn(suckerNumber,back=None):
    time.sleep(0.1)
    # ser = serial.Serial('COM5', 115200)
    log.info(f'sucker{suckerNumber} on')
    steeringEngineID={1:13,2:14,3:15,4:16,5:17}[suckerNumber]
    cylinderID={1:3,2:5,3:7,4:9,5:11}[suckerNumber]
    suckerID={1:4,2:6,3:8,4:10,5:12}[suckerNumber]
    
    # ser.write(struct.pack('iffiff',steeringEngineID,0,0,1,0,0))
    # time.sleep(0.5)
    safeSend(struct.pack('iffiff',cylinderID,0,0,0,0,0))
    time.sleep(0.1)
    safeSend(struct.pack('iffiff',suckerID,0,0,0,0,0))
    time.sleep(0.1)
    safeSend(struct.pack('iffiff',cylinderID,0,0,1,0,0))
    time.sleep(0.1)
    
    if back!=None:
        safeSend(struct.pack('iffiff',0,-200,back,0,0,0))
        safeSend(struct.pack('iffiff',steeringEngineID,0,0,0,0,0))
        time.sleep(0.2)
        safeSend(struct.pack('iffiff',0,0,0,0,0,0))
        time.sleep(0.1)
    else:
        safeSend(struct.pack('iffiff',steeringEngineID,0,0,0,0,0))
        time.sleep(0.3)
def controlSteeringEngineDown(suckerNumber,back=None):
    # time.sleep(0.1)
    # ser = serial.Serial('COM5', 115200)
    log.info(f'sucker{suckerNumber} on')
    steeringEngineID={1:13,2:14,3:15,4:16,5:17}[suckerNumber]
    cylinderID={1:3,2:5,3:7,4:9,5:11}[suckerNumber]
    suckerID={1:4,2:6,3:8,4:10,5:12}[suckerNumber]
    
    safeSend(struct.pack('iffiff',steeringEngineID,0,0,1,0,0))
    # time.sleep(0.1)
def controlSuckerOff(suckerNumber):
    # time.sleep(0.1)
    # ser = serial.Serial('COM5', 115200)
    log.info(f'sucker{suckerNumber} off')
    steeringEngineID={1:13,2:14,3:15,4:16,5:17}[suckerNumber]
    cylinderID={1:3,2:5,3:7,4:9,5:11}[suckerNumber]
    suckerID={1:4,2:6,3:8,4:10,5:12}[suckerNumber]

    safeSend(struct.pack('iffiff',cylinderID,0,0,0,0,0))
    # time.sleep(0.1)
    safeSend(struct.pack('iffiff',suckerID,0,0,1,0,0))
    # time.sleep(0.2)
    safeSend(struct.pack('iffiff',cylinderID,0,0,1,0,0))
    # time.sleep(0.1)
if __name__ == '__main__':
    # controlSteeringEngineDown(3)
    for i in range(5,6):
        controlSteeringEngineDown(i)
        time.sleep(0.5)
        controlSuckerOn(i)
        time.sleep(1)
        controlSuckerOff(i)